from launch import LaunchDescription
from launch_ros.actions import Node
# 封装终端指令相关类--------------
from launch.actions import ExecuteProcess
from launch.substitutions import FindExecutable
# 参数声明与获取-----------------
# from launch.actions import DeclareLaunchArgument
# from launch.substitutions import LaunchConfiguration
# 文件包含相关-------------------
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关----------------------
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关----------------------
# from launch.event_handlers import OnProcessStart, OnProcessExit
# from launch.actions import ExecuteProcess, RegisterEventHandler,LogInfo
# 获取功能包下share目录路径-------
from ament_index_python.packages import get_package_share_directory
import os
# 延时
from launch.actions import TimerAction
""" 
    需求：包含其他launch文件，launch嵌套。

 """
def generate_launch_description():

    #ros2 launch ardupilot_ros apm_iris_maze.launch.py rviz:=false
    apm_iris_maze = IncludeLaunchDescription(
        launch_description_source=PythonLaunchDescriptionSource(
            launch_file_path=os.path.join(
                get_package_share_directory("ardupilot_ros"),
                "launch",
                "apm_iris_maze.launch.py"
                )
            )
    )
    #Mavproxy
    Mavproxy = ExecuteProcess(
        cmd=[
                'mavproxy.py', 
                '--out', '127.0.0.1:14550',
                '--out', '127.0.0.1:14551',
                '--master', 'tcp:127.0.0.1:5760',
                '--sitl', '127.0.0.1:5501',
                '--non-interactive'
            ],
            output='screen',  # 允许在终端输出
    )
    #mavproxy = IncludeLaunchDescription(
     #   PythonLaunchDescriptionSource(
      #      [
       #         PathJoinSubstitution(
        #            [
         #               FindPackageShare("ardupilot_sitl"),
          #              "launch",
           #             "mavproxy.launch.py",
            #        ]
             #   ),
            #]
       # )
    #)
    #rqt and twist(ap/cmd_vel->cmd_vel)
    rqt_twist = IncludeLaunchDescription(
        launch_description_source=PythonLaunchDescriptionSource(
            launch_file_path=os.path.join(
                get_package_share_directory("rqt_twist"),
                "rqt_twist.launch.py"
                )
            )
    )
    #设置模式（Guided）
    #~/ros2_ws$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 15}"
    #解锁
    #~/ros2_ws$ ros2 service call /ap/arm_motors ardupilot_msgs/srv/ArmMotors "{arm: True}"
    mode_arm_service = IncludeLaunchDescription(
        launch_description_source=PythonLaunchDescriptionSource(
            launch_file_path=os.path.join(
                get_package_share_directory("mode_arm_service"),
                "mode_arm.launch.py"
                )
            )
    )
    #ros2 launch ardupilot_ros ap_cartographer.launch.py
    ap_cartographer = IncludeLaunchDescription(
        launch_description_source=PythonLaunchDescriptionSource(
            launch_file_path=os.path.join(
                get_package_share_directory("ardupilot_ros"),
                "launch",
                "ap_cartographer.launch.py"
                )
            )
    )
    return LaunchDescription([
        #1.
        apm_iris_maze,
        #2.
        TimerAction(
            period=50.0,  # 5 seconds delay
            actions=[Mavproxy]
        ),
        #3.
        TimerAction(
            period=300.0,  # 5 seconds delay
            actions=[mode_arm_service]
        ),
        #先启动rqt_twist
        TimerAction(
            period=400.0,  # 5 seconds delay
            actions=[rqt_twist]
        ),
        
        # Add delay before starting commond
        TimerAction(
            period=450.0,  # 5 seconds delay
            actions=[ap_cartographer]
        )
            ])